#include "can.h"

#include "switch_forklift.h"

//必须使用新线程来执行can的数据收发
CAN::CAN( QObject* parent ) : QObject( parent ) {
    // can发送接收周期ms
    time = 50;

#ifdef NJLAKCAN
    canDevice = new NJLikeCanDevice();
#endif
#ifdef ZLGCAN
    canDevice = new ZLGCanDevice();
#endif

#if CORNER_DRIVER == DMKE
    //舵轮转角参数-德马克
    Zero_Pulse                       = -2407666;
    pulse_per                        = 131072;  //电机一圈脉冲数值
    Veer_Wheel_Speed_Reduction_Radio = 64.0;
    pos_mode_speed                   = 1000;  //舵轮转角 rmp
    pos_mode_acc                     = 500;   // rps^2
    pos_mode_dec                     = 500;   // rps^2
#endif

#if CORNER_DRIVER == ARES
    //舵轮转角参数-ARES
    Zero_Pulse                       = 0;
    pulse_per                        = 131072;  //电机一圈脉冲数值
    Veer_Wheel_Speed_Reduction_Radio = 320;
    pos_mode_speed                   = 3000;  //舵轮转角 0~5000 rpm
    pos_mode_acc                     = 900;   // 1~1000 rps^2
    pos_mode_dec                     = 900;   // 1~1000 rps^2

#endif
}

bool CAN::CanOpenAndStart() {
    uint32 dwRel;
    dw_DeviceHandle = canDevice->CanDeviceOpen( CAN_DEVICE_TYPE, dw_Index, nullptr );
    if ( dw_DeviceHandle == 0 ) { return false; }
    // 配置CAN通道参数
    CanInitConfig config;
    config.dwAccCode  = 0;
    config.dwAccMask  = 0xffffffff;
    config.nFilter    = 0;        // 滤波方式(0表示未设置滤波功能,1表示双滤波,2表示单滤波)
    config.bMode      = 0;        // 工作模式(0表示正常模式,1表示只听模式)
    config.nBtrType   = 1;        // 位定时参数模式(1表示SJA1000,0表示LPC21XX)
    config.dwBtr[ 0 ] = dw_Btr0;  // BTR0   0014 -1M 0016-800K 001C-500K 011C-250K 031C-125K 041C-100K 091C-50K 181C-20K 311C-10K BFFF-5K
    config.dwBtr[ 1 ] = dw_Btr1;  // BTR1
    config.dwBtr[ 2 ] = 0;
    config.dwBtr[ 3 ] = 0;
    dwRel             = canDevice->CanChannelStart( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel, &config );
    if ( dwRel != CAN_DEVICE_RESULT_OK ) {
        qDebug() << "fail，dw_Channel:0";
        return false;
    }
    is_open_can = true;
    return true;
}

bool CAN::CanStopAndClose() {
    if ( is_open_can ) {
        if ( canDevice->CanChannelStopDeviceClose( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel ) ) {
            is_open_can = false;
            qDebug() << "成功，CAN关闭";
            return true;
        }
    }
    return false;
}

#if CORNER_DRIVER == DMKE
void CAN::CanDataFrameOperationalMode( CAN_DataFrame& frame, BYTE value ) {
    frame.uID           = 0;
    frame.nSendType     = 0;
    frame.bRemoteFlag   = 0;
    frame.bExternFlag   = 0;
    frame.nDataLen      = 2;
    frame.arryData[ 0 ] = value;
    frame.arryData[ 1 ] = 0;
}

void CAN::CanDataFrameSpeedMode( CAN_DataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = speed_mode[ i ];
    }
}

void CAN::CanDataFrameControlByte0F( CanDataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = enable_mode_0F[ i ];
    }
}

void CAN::CanDataFrameControlByte00( CAN_DataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = disable_mode_00[ i ];
    }
}

void CAN::CanDataFrameControlByte06( CAN_DataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = disable_mode_06[ i ];
    }
}

void CAN::CanDataFrameAcce( CAN_DataFrame& frame, BYTE drive_id, long value ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_accec[ 0 ];
    sendbuffer[ 1 ]   = set_drive_accec[ 1 ];
    sendbuffer[ 2 ]   = set_drive_accec[ 2 ];
    sendbuffer[ 3 ]   = set_drive_accec[ 3 ];
    sendbuffer[ 4 ]   = static_cast< BYTE >( value );
    sendbuffer[ 5 ]   = static_cast< BYTE >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< BYTE >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< BYTE >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void CAN::CanDataFrameDece( CAN_DataFrame& frame, BYTE drive_id, long value ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_dece[ 0 ];
    sendbuffer[ 1 ]   = set_drive_dece[ 1 ];
    sendbuffer[ 2 ]   = set_drive_dece[ 2 ];
    sendbuffer[ 3 ]   = set_drive_dece[ 3 ];
    sendbuffer[ 4 ]   = static_cast< BYTE >( value );
    sendbuffer[ 5 ]   = static_cast< BYTE >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< BYTE >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< BYTE >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void CAN::CanDataFrameSetSpeedValue( CAN_DataFrame& frame, BYTE drive_id, long value ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ] = set_speed_value[ 0 ];
    sendbuffer[ 1 ] = set_speed_value[ 1 ];
    sendbuffer[ 2 ] = set_speed_value[ 2 ];
    sendbuffer[ 3 ] = set_speed_value[ 3 ];
    sendbuffer[ 4 ] = static_cast< BYTE >( value );
    sendbuffer[ 5 ] = static_cast< BYTE >( value >> 8 );
    sendbuffer[ 6 ] = static_cast< BYTE >( value >> 16 );
    sendbuffer[ 7 ] = static_cast< BYTE >( value >> 24 );
    //设置速度
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void CAN::CanDataFrameGetCurVoltage( CAN_DataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = get_cur_voltage[ i ];
    }
}

void CAN::CanDataFrameGetCurSpeedValue( CAN_DataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = get_cur_speed[ i ];
    }
}

void CAN::CanDataFrameGetCurPoseValue( CanDataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = get_cur_pos[ i ];
    }
}

void CAN::CanDataFrameHomeMode( CAN_DataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = home_mode[ i ];
    }
}

void CAN::CanDataFrameControlByte1F( CAN_DataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = enable_mode_1F[ i ];
    }
}

void CAN::CanDataFrameControlByte2F( CAN_DataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = enable_mode_2F[ i ];
    }
}

void CAN::CanDataFrameControlByte3F( CanDataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = enable_mode_3F[ i ];
    }
}

void CAN::CanDataFramePositionMode( CAN_DataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = position_mode[ i ];
    }
}

void CAN::CanDataFrameSetPosModeSpeedValue( CAN_DataFrame& frame, BYTE drive_id, long value ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_position_mode_speed_value[ 0 ];
    sendbuffer[ 1 ]   = set_drive_position_mode_speed_value[ 1 ];
    sendbuffer[ 2 ]   = set_drive_position_mode_speed_value[ 2 ];
    sendbuffer[ 3 ]   = set_drive_position_mode_speed_value[ 3 ];
    sendbuffer[ 4 ]   = static_cast< BYTE >( value );
    sendbuffer[ 5 ]   = static_cast< BYTE >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< BYTE >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< BYTE >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void CAN::CanDataFrameSetPoModePosValue( CanDataFrame& frame, BYTE drive_id, long value ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_drive_position_mode_pos_value[ 0 ];
    sendbuffer[ 1 ]   = set_drive_position_mode_pos_value[ 1 ];
    sendbuffer[ 2 ]   = set_drive_position_mode_pos_value[ 2 ];
    sendbuffer[ 3 ]   = set_drive_position_mode_pos_value[ 3 ];
    sendbuffer[ 4 ]   = static_cast< BYTE >( value );
    sendbuffer[ 5 ]   = static_cast< BYTE >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< BYTE >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< BYTE >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void CAN::CanDataFrameGetErrInfo( CAN_DataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = get_err_info[ i ];
    }
}

void CAN::CanDataFrameNodeProtectTimer( CAN_DataFrame& frame, BYTE drive_id, long value ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_node_proetc_time[ 0 ];
    sendbuffer[ 1 ]   = set_node_proetc_time[ 1 ];
    sendbuffer[ 2 ]   = set_node_proetc_time[ 2 ];
    sendbuffer[ 3 ]   = set_node_proetc_time[ 3 ];
    sendbuffer[ 4 ]   = static_cast< BYTE >( value );
    sendbuffer[ 5 ]   = static_cast< BYTE >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< BYTE >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< BYTE >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void CAN::CanDataFrameNodeProtectFactor( CAN_DataFrame& frame, BYTE drive_id, long value ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = set_node_proetc_factor[ 0 ];
    sendbuffer[ 1 ]   = set_node_proetc_factor[ 1 ];
    sendbuffer[ 2 ]   = set_node_proetc_factor[ 2 ];
    sendbuffer[ 3 ]   = set_node_proetc_factor[ 3 ];
    sendbuffer[ 4 ]   = static_cast< BYTE >( value );
    sendbuffer[ 5 ]   = static_cast< BYTE >( value >> 8 );
    sendbuffer[ 6 ]   = static_cast< BYTE >( value >> 16 );
    sendbuffer[ 7 ]   = static_cast< BYTE >( value >> 24 );
    frame.uID         = 0x600 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void CAN::CanDataFrameRemote( CAN_DataFrame& frame, BYTE drive_id ) {
    frame.uID         = 0x700 + drive_id;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 1;
    frame.bExternFlag = 0;
    frame.nDataLen    = 0;
}
#endif

#if WALK_DRIVER == CURTIS_1232C
void CAN::CanDataFrameSetForkRaiseSpeed( CanDataFrame& frame, uchar value ) {
    frame.uID           = 0x222;
    frame.nSendType     = 0;
    frame.bRemoteFlag   = 0;
    frame.bExternFlag   = 0;
    frame.nDataLen      = 1;
    frame.arryData[ 0 ] = value;
}
void CAN::CanDataFrameSetKDSData( CanDataFrame& frame, short& speed, uchar& kdsRise, uchar& kdsFall ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = static_cast< BYTE >( speed );
    sendbuffer[ 1 ]   = static_cast< BYTE >( speed >> 8 );
    sendbuffer[ 2 ]   = 0;
    sendbuffer[ 3 ]   = 0;
    sendbuffer[ 4 ]   = kdsRise;
    sendbuffer[ 5 ]   = kdsFall;
    sendbuffer[ 6 ]   = 0;
    sendbuffer[ 7 ]   = 0;
    frame.uID         = 0x226;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}
#endif

#if WALK_DRIVER == CURTIS_F2A
void CAN::CanDataFrameSetForkContorl( CanDataFrame& frame, uchar& lift_enable, short& fork_lft_speed, short& kds_fall ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = lift_enable;
    sendbuffer[ 1 ]   = 0;
    sendbuffer[ 2 ]   = 0;
    sendbuffer[ 3 ]   = fork_lft_speed;
    sendbuffer[ 4 ]   = static_cast< BYTE >( kds_fall );
    sendbuffer[ 5 ]   = static_cast< BYTE >( kds_fall >> 8 );
    frame.uID         = 0x313;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 6;
    for ( int i = 0; i < 6; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void CAN::CanDataFrameSetKDSWalkData( CanDataFrame& frame, uchar& Direction_enable, short& speed, short& acc, short& dec, short& max_acc, short& max_dec, short& brake_strength ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = Direction_enable;
    sendbuffer[ 1 ]   = static_cast< BYTE >( speed );
    sendbuffer[ 2 ]   = static_cast< BYTE >( speed >> 8 );
    sendbuffer[ 3 ]   = acc;
    sendbuffer[ 4 ]   = dec;
    sendbuffer[ 5 ]   = max_acc;
    sendbuffer[ 6 ]   = max_dec;
    sendbuffer[ 7 ]   = brake_strength;
    frame.uID         = 0x213;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}
#endif

void CAN::ARES_init() {
    CanDataFrame frames[ 20 ];
    ULONG        i = 0;
    // T曲线
    frames[ i ].uID           = 0x601;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x2B;
    frames[ i ].arryData[ 1 ] = 0x86;
    frames[ i ].arryData[ 2 ] = 0x60;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = 0x00;
    frames[ i ].arryData[ 5 ] = 0x00;
    i++;
    // 最大速度
    frames[ i ].uID           = 0x601;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x2B;
    frames[ i ].arryData[ 1 ] = 0x7F;
    frames[ i ].arryData[ 2 ] = 0x60;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = ( BYTE )( pos_mode_speed & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( pos_mode_speed >> 8 ) & 0xFF );
    // 编码器分辨率
    frames[ i ].uID           = 0x601;
    frames[ i ].nDataLen      = 8;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x23;
    frames[ i ].arryData[ 1 ] = 0x8F;
    frames[ i ].arryData[ 2 ] = 0x60;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = ( BYTE )( pulse_per & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( pulse_per >> 8 ) & 0xFF );
    frames[ i ].arryData[ 6 ] = ( BYTE )( ( pulse_per >> 16 ) & 0xFF );
    frames[ i ].arryData[ 7 ] = ( BYTE )( ( pulse_per >> 24 ) & 0xFF );
    i++;
    // 最大加速度
    frames[ i ].uID           = 0x601;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x2B;
    frames[ i ].arryData[ 1 ] = 0x82;
    frames[ i ].arryData[ 2 ] = 0x60;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = ( BYTE )( pos_mode_acc & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( pos_mode_acc >> 8 ) & 0xFF );
    i++;
    // 最大减速度
    frames[ i ].uID           = 0x601;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x2B;
    frames[ i ].arryData[ 1 ] = 0x83;
    frames[ i ].arryData[ 2 ] = 0x60;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = ( BYTE )( pos_mode_dec & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( pos_mode_dec >> 8 ) & 0xFF );
    i++;
    //发送06电机通电
    frames[ i ].uID           = 0x201;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x06;
    frames[ i ].arryData[ 1 ] = 0x00;
    frames[ i ].arryData[ 2 ] = 0x00;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = 0x00;
    frames[ i ].arryData[ 5 ] = 0x00;
    i++;
    //发送07电机使能
    frames[ i ].uID           = 0x201;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x07;
    frames[ i ].arryData[ 1 ] = 0x00;
    frames[ i ].arryData[ 2 ] = 0x00;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = 0x00;
    frames[ i ].arryData[ 5 ] = 0x00;
    i++;
    CanSendData( frames, i );
    emit sigSendTips( "转角电机初始化" );
}

bool CAN::CanRecviveData() {
    if ( !is_open_can ) { return false; }
    CanDataFrame vco[ 200 ];
    uint         dwRel = static_cast< uint >( canDevice->CanChannelReceive( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel, vco, 200, 40 ) );
    if ( dwRel <= 0 ) { return false; }
    for ( ULONG i = 0; i < dwRel; i++ ) {
        QByteArray recv;
        for ( int k = 0; k < vco[ i ].nDataLen; k++ ) {
            recv.append( static_cast< char >( vco[ i ].arryData[ k ] ) );
        }
        emit sigShowCanData( recv, "Recv1:" + QString::number( vco[ i ].uID, 16 ) );  //发送打印static_cast<short int>

        if ( vco[ i ].uID == 0X1A6 ) {
            // rpm
            short int recive_walk_speed = static_cast< short int >( ( vco[ i ].arryData[ 0 ] ) + ( vco[ i ].arryData[ 1 ] << 8 ) );
            // m/s
            fork_walk_reality_speed = static_cast< double >( recive_walk_speed ) / Walk_Wheel_Speed_Reduction_Radio / 60 * ( 2 * M_PI * Walk_Wheel_R );
            //            qDebug()<<"whell speed:"<<fork_walk_reality_speed<<endl;
        }
#if CORNER_DRIVER == ARES
        // ARES 舵轮脉冲反馈
        else if ( vco[ i ].uID == 0x181 ) {
            ARES_status_word      = vco[ i ].arryData[ 0 ] + ( vco[ i ].arryData[ 1 ] << 8 );
            int value             = vco[ i ].arryData[ 4 ] + ( vco[ i ].arryData[ 5 ] << 8 ) + ( vco[ i ].arryData[ 6 ] << 16 ) + ( vco[ i ].arryData[ 7 ] << 24 );
            fork_veer_reality_rad = static_cast< double >( value - Zero_Pulse ) / pulse_per / Veer_Wheel_Speed_Reduction_Radio * ( 2 * M_PI );
        }
        // ARES AUTO CHANGE
        else if ( vco[ i ].uID == 0x381 ) {
            static bool flag = false;
            if ( ( vco[ i ].arryData[ 0 ] & 0b1 ) == 0b0 ) {  // 拨到了自动
                if ( !flag /*((ARES_status_word >> 1) & 0b1) == 0b0*/ ) {
                    ARES_init();
                    flag = true;
                }
            }
            else {  // 拨到了手动
                flag = false;
            }
        }
#endif
#if CORNER_DRIVER == DMKE
        // DMKE 舵轮脉冲反馈
        else if ( vco[ i ].uID == static_cast< unsigned int >( 0x580 + turn_drive_id ) ) {
            if ( ( vco[ i ].nDataLen == 8 ) && ( vco[ i ].arryData[ 0 ] == 0x43 ) && ( vco[ i ].arryData[ 1 ] == 0x64 ) && ( vco[ i ].arryData[ 2 ] == 0x60 ) && ( vco[ i ].arryData[ 3 ] == 0x00 ) ) {
                int recive_veer_puls = ( vco[ i ].arryData[ 4 ] ) + ( vco[ i ].arryData[ 5 ] << 8 ) + ( vco[ i ].arryData[ 6 ] << 16 ) + ( vco[ i ].arryData[ 7 ] << 24 );
                //（脉冲差值/一圈脉冲数）*2π /减速比
                fork_veer_reality_rad = -static_cast< double >( recive_veer_puls - Zero_Pulse ) / pulse_per * ( 2 * M_PI ) / Veer_Wheel_Speed_Reduction_Radio;
            }
        }

#endif
    }
    return true;
}

void CAN::sltCanInit() {
    if ( CanOpenAndStart() ) {  // open can drive
        emit sigSendTips( "正确，CAN1设备打开成功" );
    }
    else {
        emit sigSendTips( "错误，CAN1设备打开失败" );
        return;
    }
    ULONG        i = 0;
    CanDataFrame frames[ 20 ];
    ARES_init();

    /******行走轮设定速度*******/
    CanDataFrameSetKDSData( frames[ i++ ], kds_speed, kds_rise, kds_fall );
    CanSendData( frames, i );
    emit sigSendTips( "提示，驱动电机初始化完成！" );
    Utility::delayMs( 1000 );  // 非阻塞延时执行
    QTimer* timer50ms = new QTimer();
    connect( timer50ms, SIGNAL( timeout() ), this, SLOT( sltTimeOut() ) );
    timer50ms->start( time );
}

void CAN::CanSendData( CanDataFrame* frames, ULONG i ) {
    if ( i == 0 ) return;
    if ( FRAMES_CNT >= i )
        canDevice->CanChannelSend( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel, frames, i );
    else {
        for ( ; i > FRAMES_CNT; i -= FRAMES_CNT )
            canDevice->CanChannelSend( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel, frames, FRAMES_CNT );
        if ( i > 0 ) canDevice->CanChannelSend( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel, frames + FRAMES_CNT, i );
    }
    for ( uint count = 0; count < i; count++ ) {
        QByteArray recv;
        for ( int k = 0; k < frames[ count ].nDataLen; k++ ) {
            recv.append( static_cast< char >( frames[ count ].arryData[ k ] ) );
        }
        emit sigShowCanData( recv, "Send:" + QString::number( frames[ count ].uID, 16 ) );  //发送打印static_cast<short int>
    }
}

void CAN::RegularGetData() {
    ULONG        i = 0;
    CanDataFrame frames[ 20 ];
    CanDataFrameSetForkRaiseSpeed( frames[ i++ ], fork_lft_speed );
    CanDataFrameSetKDSData( frames[ i++ ], kds_speed, kds_rise, kds_fall );
#if CORNER_DRIVER == DMKE
    /***********DMKE Angle Set*************/
    CanDataFrameGetCurPoseValue( frames[ i++ ], turn_drive_id );
    CanDataFrameSetPoModePosValue( frames[ i++ ], turn_drive_id, fork_calc_puls );
    CanDataFrameControlByte0F( frames[ i++ ], turn_drive_id );
    CanDataFrameControlByte3F( frames[ i++ ], turn_drive_id );
#endif
#if CORNER_DRIVER == ARES
    /***********ARES Angle Set*************/
    frames[ i ].uID           = 0x201;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x0F;
    frames[ i ].arryData[ 1 ] = 0x00;
    frames[ i ].arryData[ 2 ] = ( BYTE )( fork_calc_puls & 0xFF );
    frames[ i ].arryData[ 3 ] = ( BYTE )( ( fork_calc_puls >> 8 ) & 0xFF );
    frames[ i ].arryData[ 4 ] = ( BYTE )( ( fork_calc_puls >> 16 ) & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( fork_calc_puls >> 24 ) & 0xFF );
    i++;
    frames[ i ].uID           = 0x201;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x5F;
    frames[ i ].arryData[ 1 ] = 0x00;
    frames[ i ].arryData[ 2 ] = ( BYTE )( fork_calc_puls & 0xFF );
    frames[ i ].arryData[ 3 ] = ( BYTE )( ( fork_calc_puls >> 8 ) & 0xFF );
    frames[ i ].arryData[ 4 ] = ( BYTE )( ( fork_calc_puls >> 16 ) & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( fork_calc_puls >> 24 ) & 0xFF );
    i++;
#endif
    CanSendData( frames, i );
}
void CAN::sltTimeOut() {
    if ( !is_open_can ) { return; }
    //定期发送
    RegularGetData();
    //定期接收
    CanRecviveData();
}

// send ros
void CAN::GetCanRecivNaviParam( double* walkspeed, double* veerad ) {
    if ( !is_open_can ) { return; }
    *walkspeed = fork_walk_reality_speed;  //叉车直行速度反馈
    *veerad    = fork_veer_reality_rad;    //叉车转角弧度反馈
}

void CAN::CanSetSpeedAngle( double walk_speed, double veer_rad ) {

    fork_walk_setting_speed = walk_speed;
    fork_veer_setting_rad   = veer_rad;
    // 速度m/s = 给定速度（Walk_Wheel_Speed）* 60 / 周长（Walk_Wheel_C） *
    // 减速比（Walk_Wheel_Speed_Reduction_Radio）
    kds_speed = static_cast< short >( ( fork_walk_setting_speed * 60 / Walk_Wheel_C ) * Walk_Wheel_Speed_Reduction_Radio );
#if CORNER_DRIVER == DMKE
    // DMKE 脉冲值 = 弧度
    fork_calc_puls = static_cast< long >( -fork_veer_setting_rad * Veer_Wheel_Speed_Reduction_Radio * pulse_per / ( 2 * M_PI ) + Zero_Pulse );
#endif
#if CORNER_DRIVER == ARES
    // ARES 脉冲值 = 弧度
    fork_calc_puls = static_cast< long >( fork_veer_setting_rad * Veer_Wheel_Speed_Reduction_Radio * pulse_per / ( 2 * M_PI ) + Zero_Pulse );
#endif
}

void CAN::AccDecUpdate( double& acc, double& dec ) {
    Q_UNUSED( acc );
    Q_UNUSED( dec );
    // todo
}

void CAN::GetDriveErrInfo() {}

void CAN::push_fork_upper() {

    kds_fall       = 0;
    kds_rise       = 1;
    fork_lft_speed = 100;
}

void CAN::push_fork_stop() {
    kds_fall       = 0;
    kds_rise       = 0;
    fork_lft_speed = 0;
}

void CAN::push_fork_down() {

    kds_fall       = 200;
    kds_rise       = 150;
    fork_lft_speed = 0;
}
